// LibDepthSmoother.cpp : Defines the exported functions for the DLL application.
//

#include "stdafx.h"
#include <stdlib.h>
#include <Windows.h>

#define DllExport extern "C" __declspec(dllexport)

#define MaxDepthDistance 4000
#define MinDepthDistance 800
#define MaxDepthDistanceOffset 3150

static int InnerBandThreshold = 1;
static int OuterBandThreshold = 1;

static inline short CalculateDistanceFromDepth(unsigned char first, unsigned char second)
{
    // Please note that this would be different if you use Depth and User tracking rather than just depth
    return (first | second << 8);
}

static inline unsigned char CalculateIntensityFromDistance(int distance)
{
    // This will map a distance value to a 0 - 255 range
    // for the purposes of applying the resulting value
    // to RGB pixels.
    int newMax = distance - MinDepthDistance;
    if (newMax > 0)
        return (unsigned char)(255 - (255 * newMax
        / ( MaxDepthDistanceOffset )));
    else
        return (unsigned char)255;
}

DllExport void _cdecl CreateDepthArray(unsigned char * depthFrame, short * depthArray)
{
	// When creating a depth array, it will have half the number of indexes than the original depth image
    // This is because the depth image uses two bytes to represent depth.  These values must then be 
    // transformed to a single value per pixel of the final image that represents depth
    // for purposes of smoothing prior to rendering.

    for (int depthImageRowIndex = 0; depthImageRowIndex < 240; depthImageRowIndex++)
    {
        for (int depthImageColumnIndex = 0; depthImageColumnIndex < 640; depthImageColumnIndex += 2)
        {
            int depthIndex = depthImageColumnIndex + (depthImageRowIndex * 640);
            
            depthArray[depthIndex / 2] = CalculateDistanceFromDepth(depthFrame[depthIndex], depthFrame[depthIndex + 1]);
        }
    }
}

int* filterCollection = new int[24, 3];

DllExport void _cdecl CreateFilteredDepthArray(short *depthArray, short *filteredDepthArray)
{
	int width = 320, height = 240;

    // We will be using these numbers for constraints on indexes
    int widthBound = width - 1;
    int heightBound = height - 1;

    // Process each row
    for(int depthArrayRowIndex = 0; depthArrayRowIndex < height; depthArrayRowIndex++)
    {
		int x, y, innerBandCount, depthIndex, outerBandCount, row, xi, yi, xSearch, ySearch, index, depth, frequency, i;
		
        // Process each pixel in the row
        for (int depthArrayColumnIndex = 0; depthArrayColumnIndex < width; depthArrayColumnIndex++)
        {
			for(row = 0; row < 24; row++)
			{
				filterCollection[row, 0] = 0;
				filterCollection[row, 1] = 0;
				filterCollection[row, 2] = 0;
			}

            depthIndex = depthArrayColumnIndex + (depthArrayRowIndex * width);

            // We are only concerned with eliminating 'white' noise from the data.
            // We consider any pixel with a depth of 0 as a possible candidate for filtering.
            if (depthArray[depthIndex] == 0)
            {
                // From the depth index, we can determine the X and Y coordinates that the index
                // will appear in the image.  We use this to help us define our filter matrix.
                x = depthIndex % width;
                y = (depthIndex - x) / width;

                // The inner and outer band counts are used later to compare against the threshold 
                // values set in the UI to identify a positive filter result.
                innerBandCount = 0;
                outerBandCount = 0;

                // The following loops will loop through a 5 X 5 matrix of pixels surrounding the 
                // candidate pixel.  This defines 2 distinct 'bands' around the candidate pixel.
                // If any of the pixels in this matrix are non-0, we will accumulate them and count
                // how many non-0 pixels are in each band.  If the number of non-0 pixels breaks the
                // threshold in either band, then the average of all non-0 pixels in the matrix is applied
                // to the candidate pixel.
                for (yi = -2; yi < 3; yi++)
                {
                    for (xi = -2; xi < 3; xi++)
                    {
                        // yi and xi are modifiers that will be subtracted from and added to the
                        // candidate pixel's x and y coordinates that we calculated earlier.  From the
                        // resulting coordinates, we can calculate the index to be addressed for processing.

                        // We do not want to consider the candidate pixel (xi = 0, yi = 0) in our process at this point.
                        // We already know that it's 0
                        if (xi != 0 || yi != 0)
                        {
                            // We then create our modified coordinates for each pass
                            xSearch = x + xi;
                            ySearch = y + yi;

                            // While the modified coordinates may in fact calculate out to an actual index, it 
                            // might not be the one we want.  Be sure to check to make sure that the modified coordinates
                            // match up with our image bounds.
                            if (xSearch >= 0 && xSearch <= widthBound && ySearch >= 0 && ySearch <= heightBound)
                            {
                                index = xSearch + (ySearch * width);
                                // We only want to look for non-0 values
                                if (depthArray[index] != 0)
                                {
                                    // We want to find count the frequency of each depth
                                    for (i = 0; i < 24; i++)
                                    {
                                        if (filterCollection[i, 0] == depthArray[index])
                                        {
                                            // When the depth is already in the filter collection
                                            // we will just increment the frequency.
                                            filterCollection[i, 1]++;
                                            break;
                                        }
                                        else if (filterCollection[i, 0] == 0)
                                        {
                                            // When we encounter a 0 depth in the filter collection
                                            // this means we have reached the end of values already counted.
                                            // We will then add the new depth and start it's frequency at 1.
                                            filterCollection[i, 0] = depthArray[index];
                                            filterCollection[i, 1]++;
                                            break;
                                        }
                                    }

                                    // We will then determine which band the non-0 pixel
                                    // was found in, and increment the band counters.
                                    if (yi != 2 && yi != -2 && xi != 2 && xi != -2)
                                        innerBandCount++;
                                    else
                                        outerBandCount++;
                                }
                            }
                        }
                    }
                }

                // Once we have determined our inner and outer band non-zero counts, and accumulated all of those values,
                // we can compare it against the threshold to determine if our candidate pixel will be changed to the
                // statistical mode of the non-zero surrounding pixels.
                if (innerBandCount >= InnerBandThreshold || outerBandCount >= OuterBandThreshold)
                {
                    frequency = 0;
                    depth = 0;
                    // This loop will determine the statistical mode
                    // of the surrounding pixels for assignment to
                    // the candidate.
                    for (i = 0; i < 24; i++)
                    {
                        // This means we have reached the end of our
                        // frequency distribution and can break out of the
                        // loop to save time.
                        if (filterCollection[i, 0] == 0)
                            break;
                        if (filterCollection[i, 1] > frequency)
                        {
                            depth = filterCollection[i, 0];
                            frequency = filterCollection[i, 1];
                        }
                    }

                    filteredDepthArray[depthIndex] = depth;
                }

            }
            else
            {
                // If the pixel is not zero, we will keep the original depth.
				short tmp = depthArray[depthIndex];
                filteredDepthArray[depthIndex] = tmp;
            }
        }
	}
}

DllExport void _cdecl SetParams(int innerBand, int outerBand)
{
	InnerBandThreshold = innerBand;
	OuterBandThreshold = outerBand;
}